//*************************************************************************************************
//************************ (C) 2001 - 2006, Eleven Engineering Incorporated ***********************
//*************************************************************************************************
//**
//**		Tabs:	This file looks best with tab stops set every 6 spaces.
//**
//*************************************************************************************************
//*************************************************************************************************
//**
//**          File:  Debug.asm
//**       Project:  XInc2 Library
//**       Created:  04 Jan 2003 by Ryan Northcott
//**       Revised:  08 Sep 2006 by Dan Beutel
//**
//**   Description:  Some useful routines for debugging code in realtime.
//**
//** Compatibility:  Requires XInA v4.4 or higher.
//**
//**    Disclaimer:  You may incorporate this sample source code into your program(s) without
//**                 restriction.  This sample source code has been provided "AS IS" and the
//**                 responsibility for its operation is yours.  You are not permitted to
//**                 redistribute this sample source code as "Eleven sample source code" after
//**                 having made changes.  If you are going to re-distribute the source, we require
//**                 that you make it clear in the source that the code was descended from Eleven
//**                 sample source code, but that you have made changes.
//**
//*************************************************************************************************
//*************************************************************************************************
//**
//**	FatalError
//**	StopAllThreadsButMe
//**
//*************************************************************************************************
//*************************************************************************************************


#ifndef __DEBUG_UTILS__
#define __DEBUG_UTILS__

#include "XPD Echo.asm"

//=============================================================================
// Input Params:	Store r6 to sp+2 immediately before calling this routine
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Echos an error message, displays the PCs and
//			Registers of all threads in a table, and flashes the
//			development board's LEDs.  Call this routine when
//			you detect some condition in your code that is illegal.
//=============================================================================
FatalError:
		sub	r6, r6, 2	// The Calling Address is 2 words above the Return Address
		st	r6, sp, 0	// sp -9 (Calling Address)
		st	r5, sp, 3	// sp -6 (Original r5)

		// stop all threads but me
		thrd	r6
		mov	r5, 0xFFFE
		rol	r5, r5, r6
		outp	r5, SCUstop
		
		st	r7, sp, 1	// sp -8 (Original r7)
		//st	r6, sp, 2	// sp -7 (Original r6) // Saved on the stack before this subroutine is called
		//st	r5, sp, 3	// sp -6 (Original r5) // Stored earlier
		st	r4, sp, 4	// sp -5 (Original r4)
		st	r3, sp, 5	// sp -4 (Original r3)
		st	r2, sp, 6	// sp -3 (Original r2)
		st	r1, sp, 7	// sp -2 (Original r1)
		st	r0, sp, 8	// sp -1 (Original r0)
		add	sp, sp, 9
		
	FatalError_MainLoop:
		
		mov	r1, MSG_NEWLINE
		jsr	r6, XPD_EchoString

	FatalError_DisplayErrorCode:

		mov	r1, _DEBUG_MSG_STRING1
		jsr	r6, XPD_EchoString
		
		thrd	r1
		jsr	r6, XPD_EchoUnsignedDecNLZ
		
		mov	r1, _DEBUG_MSG_STRING2
		jsr	r6, XPD_EchoString
		
		ld	r1, sp, -9
		jsr	r6, XPD_EchoHex

		mov	r1, MSG_NEWLINE
		jsr	r6, XPD_EchoString
		
	//---------------------------
	// Print Out Thread Table Header
		mov	r1, MSG_4SPACES
		jsr	r6, XPD_EchoString
		
		mov	r2, 7
	FatalError_TableHeader_loop:
		mov	r1, _DEBUG_MSG_T
		jsr	r6, XPD_EchoString
		
		add	r1, r2, 0
		jsr	r6, XPD_EchoUnsignedDecNLZ
		
		mov	r1, MSG_6SPACES
		jsr	r6, XPD_EchoString
		
		sub	r2, r2, 1
		bc	NC, FatalError_TableHeader_loop

		mov	r1, MSG_NEWLINE
		jsr	r6, XPD_EchoString
	
	//---------------------------
	// Print Out All Thread PCs
	
		mov	r1, _DEBUG_MSG_PC
		jsr	r6, XPD_EchoString
	
		mov	r2, 7
	FatalError_PCLoop:
		thrd	r3
		sub	r3, r3, r2
		bc	EQ, FatalError_PCLoop_CallingThread
	
		rol	r4, r2, 3		// shift left 3 places
		outp	r4, SCUpntr
		mov	r1, 0			// stall 1 tick
		inp	r1, SCUpc		// r1 = pc
		
		st	r2, sp, 0
		add	sp, sp, 1
		jsr	r6, XPD_EchoHex
		sub	sp, sp, 1
		ld	r2, sp, 0
		
		mov	r1, MSG_2SPACES
		st	r2, sp, 0
		add	sp, sp, 1
		jsr	r6, XPD_EchoString
		sub	sp, sp, 1
		ld	r2, sp, 0
	
	FatalError_PCLoop_NextLoop:
		sub	r2, r2, 1
		bc	NC, FatalError_PCLoop
		bra	FatalError_PCLoop_LoopEND
		
	FatalError_PCLoop_CallingThread:
		ld	r1, sp, -9
		jsr	r6, XPD_EchoHex
		
		mov	r1, MSG_2SPACES
		jsr	r6, XPD_EchoString
		
		bra	FatalError_PCLoop_NextLoop
	
	FatalError_PCLoop_LoopEND:
		mov	r1, MSG_NEWLINE
		jsr	r6, XPD_EchoString
	
	//---------------------------
	// Print Out All Thread Registers
		
		mov	r0, 0			// r0 = Register #
	FatalError_RegLoop0:
		mov	r1, _DEBUG_MSG_R
		jsr	r6, XPD_EchoString

		add	r1, r0, '0'
		jsr	r6, XPD_WriteByte
		
		mov	r1, MSG_2SPACES
		jsr	r6, XPD_EchoString
		
		mov	r2, 7			// r2 = Thread #
		thrd	r3
	FatalError_RegLoop1:
		sub	r5, r3, r2
		bc	EQ, FatalError_RegLoop2_PrintStoredReg
		
		rol	r4, r2, 3		// shift left 3 places
		add	r5, r4, r0
		outp	r5, SCUpntr
		inp	r1, SCUreg		// r1 = reg
		jsr	r6, XPD_EchoHex
		bra	FatalError_RegLoop1_next
	
	FatalError_RegLoop2_PrintStoredReg:
		sub	r1, sp, r0
		ld	r1, r1, -1
		jsr	r6, XPD_EchoHex
	
	FatalError_RegLoop1_next:
		mov	r1, MSG_2SPACES
		jsr	r6, XPD_EchoString

		sub	r2, r2, 1
		bc	NC, FatalError_RegLoop1
		
	FatalError_RegLoop_NextLoop:
		mov	r1, MSG_NEWLINE
		jsr	r6, XPD_EchoString
		
		add	r0, r0, 1
		sub	r5, r0, 8
		bc	ZC, FatalError_RegLoop0
		
	FatalError_LEDState1:
		mov	r1, 0xAAAA
		jsr	r6, SetLEDs
		
		jsr	r6, DelayLong
		jsr	r6, DelayLong
		jsr	r6, DelayLong
	
		FatalError_LEDState2:
		mov	r1, 0x5555
		jsr	r6, SetLEDs
		
		jsr	r6, DelayLong
		jsr	r6, DelayLong
		jsr	r6, DelayLong

		bra	FatalError_MainLoop



//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Stops all threads except for the thread that is executing
//			this routine.
//=============================================================================
StopAllThreadsButMe:
		st	r2, sp, 0
		st	r3, sp, 1
					
		thrd	r3
		mov	r2, 0xFFFE
		rol	r2, r2, r3
		outp	r2, SCUstop
					
StopAllThreadsButMe_END:
		ld	r2, sp, 0
		ld	r3, sp, 1
		jsr	r6, r6



//=============================================================================
// Fatal Error Messages
//
// NOTE:	It is OK to have this data in the code space since it will never be
//		accessed when more than one thread is running.
//=============================================================================

_DEBUG_MSG_STRING1:
	"FATAL ERROR ON THREAD ", EOS
_DEBUG_MSG_STRING2:
	" AT ADDRESS ", EOS
_DEBUG_MSG_PC:
	"PC  ", EOS
_DEBUG_MSG_R:
	"R", EOS
_DEBUG_MSG_T:
	"T", EOS

#endif